#ifndef GEOMETRY_H_INCLUDED
#define GEOMETRY_H_INCLUDED

#include <math.h>

typedef float VECTOR3[3];
typedef float VECTOR4[4];
typedef float MATRIX3[9];
typedef float MATRIX4[16];

void sqsMultMatrix4(MATRIX4 dest, MATRIX4 m1, MATRIX4 m2);
void sqsMultMat4Vec4(VECTOR4 dest, MATRIX4 m, VECTOR4 v);
void sqsMultMat4Vec3(VECTOR3 dest, MATRIX4 m, VECTOR3 v);
float sqsNormVector3(VECTOR3 v);
float sqsNormVector4(VECTOR4 v);
void sqsNormalizeVector3(VECTOR3 dest, VECTOR3 src);
void sqsNormalizeVector4(VECTOR4 dest, VECTOR4 src);
float sqsRadianos(float graus);
int sqsInverseMatrix4(MATRIX4 dest, MATRIX4 src);

#define X 0
#define Y 1
#define Z 2
#define W 3

#define sqsSETVECTOR3(destino, a, b, c){   \
    (destino)[0] = a;  \
    (destino)[1] = b;  \
    (destino)[2] = c;  }

#define sqsSETVECTOR4(destino, a, b, c, d){   \
    (destino)[0] = a;  \
    (destino)[1] = b;  \
    (destino)[2] = c;  \
    (destino)[3] = d;}

#define sqsSETMATRIX3(destino, a, b, c, d, e, f, g, h, i) {   \
  (destino)[ 0] = a; (destino)[ 1] = b; (destino)[ 2] = c; \
  (destino)[ 3] = d; (destino)[ 4] = e; (destino)[ 5] = f; \
  (destino)[ 6] = g; (destino)[ 7] = h; (destino)[ 8] = i; }

#define sqsSETMATRIX4(destino, a, b, c, d, e, f, g, h, i, j, k, l, m, n, o, p) {   \
  (destino)[ 0] = a; (destino)[ 1] = b; (destino)[ 2] = c; (destino)[ 3] = d; \
  (destino)[ 4] = e; (destino)[ 5] = f; (destino)[ 6] = g; (destino)[ 7] = h; \
  (destino)[ 8] = i; (destino)[ 9] = j; (destino)[10] = k; (destino)[11] = l; \
  (destino)[12] = m; (destino)[13] = n; (destino)[14] = o; (destino)[15] = p; }



#define sqsIDENTITYMATRIX4(m){    \
    (m)[0] = (m)[5] = (m)[10] = (m)[15] = 1.0f;   \
    (m)[1] = (m)[2] = (m)[3] =   \
    (m)[4] = (m)[6] = (m)[7] =   \
    (m)[8] = (m)[9] = (m)[11] =  \
    (m)[12] = (m)[13] = (m)[14] = 0.0f; }

#define sqsIDENTITYMATRIX3(m){    \
    (m)[0] = (m)[4] = (m)[8] = 1.0f;   \
    (m)[1] = (m)[2] =   \
    (m)[3] = (m)[5] =   \
    (m)[6] = (m)[7] = 0.0f; }

#define sqsMULTMATRIX3(destino,m1,m2){  \
        (destino) [0]  = (m1)[0]  * (m2)[0] + (m1)[1] * (m2)[3] + (m1)[2] * (m2)[6];  \
        (destino) [3]  = (m1)[3]  * (m2)[0] + (m1)[4] * (m2)[3] + (m1)[5] * (m2)[6];  \
        (destino) [6]  = (m1)[6]  * (m2)[0] + (m1)[7] * (m2)[3] + (m1)[8] * (m2)[6];  \
        (destino) [1]  = (m1)[0]  * (m2)[1] + (m1)[1] * (m2)[4] + (m1)[2] * (m2)[7];  \
        (destino) [4]  = (m1)[3]  * (m2)[1] + (m1)[4] * (m2)[4] + (m1)[5] * (m2)[7];  \
        (destino) [7]  = (m1)[6]  * (m2)[1] + (m1)[7] * (m2)[4] + (m1)[8] * (m2)[7];  \
        (destino) [2]  = (m1)[0]  * (m2)[2] + (m1)[1] * (m2)[5] + (m1)[2] * (m2)[8];  \
        (destino) [5]  = (m1)[3]  * (m2)[2] + (m1)[4] * (m2)[5] + (m1)[5] * (m2)[8];  \
        (destino) [8]  = (m1)[6]  * (m2)[2] + (m1)[7] * (m2)[5] + (m1)[8] * (m2)[8];}

#define sqsTRANSPOSEMTARIX4(destino,origem){  \
        (destino) [0] = (origem) [0];  \
        (destino) [1] = (origem) [4];  \
        (destino) [2] = (origem) [8];  \
        (destino) [3] = (origem) [12];  \
        (destino) [4] = (origem) [1];  \
        (destino) [5] = (origem) [5];  \
        (destino) [6] = (origem) [9];  \
        (destino) [7] = (origem) [13];  \
        (destino) [8] = (origem) [2];  \
        (destino) [9] = (origem) [6];  \
        (destino) [10] = (origem) [10];  \
        (destino) [11] = (origem) [14];  \
        (destino) [12] = (origem) [3];  \
        (destino) [13] = (origem) [7];  \
        (destino) [14] = (origem) [11];  \
        (destino) [15] = (origem) [15];}

#define sqsCOPYVECTOR3(destino,origem){  \
        (destino) [0] = (origem) [0];  \
        (destino) [1] = (origem) [1];  \
        (destino) [2] = (origem) [2];}

#define sqsCOPYVECTOR4(destino,origem){  \
        (destino) [0] = (origem) [0];  \
        (destino) [1] = (origem) [1];  \
        (destino) [2] = (origem) [2];  \
        (destino) [3] = (origem) [3];}

#define sqsCOPYMATRIX4(destino,origem){  \
        (destino) [0] = (origem) [0];  \
        (destino) [1] = (origem) [1];  \
        (destino) [2] = (origem) [2];  \
        (destino) [3] = (origem) [3];  \
        (destino) [4] = (origem) [4];  \
        (destino) [5] = (origem) [5];  \
        (destino) [6] = (origem) [6];  \
        (destino) [7] = (origem) [7];  \
        (destino) [8] = (origem) [8];  \
        (destino) [9] = (origem) [9];  \
        (destino) [10] = (origem) [10];  \
        (destino) [11] = (origem) [11];  \
        (destino) [12] = (origem) [12];  \
        (destino) [13] = (origem) [13];  \
        (destino) [14] = (origem) [14];  \
        (destino) [15] = (origem) [15];}

#define sqsCOPYMATRIX3(destino,origem){  \
        (destino) [0] = (origem) [0];  \
        (destino) [1] = (origem) [1];  \
        (destino) [2] = (origem) [2];  \
        (destino) [3] = (origem) [3];  \
        (destino) [4] = (origem) [4];  \
        (destino) [5] = (origem) [5];  \
        (destino) [6] = (origem) [6];  \
        (destino) [7] = (origem) [7];  \
        (destino) [8] = (origem) [8];}

#define sqsCOPYMATRIX3_4(destino,origem){  \
        (destino) [0] = (origem) [0];  \
        (destino) [1] = (origem) [1];  \
        (destino) [2] = (origem) [2];  \
        (destino) [4] = (origem) [3];  \
        (destino) [5] = (origem) [4];  \
        (destino) [6] = (origem) [5];  \
        (destino) [8] = (origem) [6];  \
        (destino) [9] = (origem) [7];  \
        (destino) [10] = (origem) [8]; \
        (destino) [3] = (destino) [7] = (destino) [11] = (destino) [12] = (destino) [13] = (destino) [14] =  0.0f;  \
        (destino) [15] =  1.0f;}

#define sqsDOTPRODUCT4(destino,v1,v2){  \
        (destino) = (v1)[0]*(v2)[0] + (v1)[1]*(v2)[1] + (v1)[2]*(v2)[2] + (v1)[3]*(v2)[3];}


#define sqsDOTPRODUCT3(destino,v1,v2){  \
        (destino) = (v1)[0]*(v2)[0] + (v1)[1]*(v2)[1] + (v1)[2]*(v2)[2];}

#define sqsCROSSPRDUCT3(destino,v1,v2){  \
        (destino) [0] = (v1)[1]*(v2)[2] - (v1)[2]*(v2)[1];  \
        (destino) [1] = (v1)[0]*(v2)[2] - (v1)[2]*(v2)[0];  \
        (destino) [2] = (v1)[0]*(v2)[1] - (v1)[1]*(v2)[0];}

#define sqsADDVECTOR4(destino,v1,v2){  \
        (destino)[0] = (v1)[0] + (v2)[0];  \
        (destino)[1] = (v1)[1] + (v2)[1];  \
        (destino)[2] = (v1)[2] + (v2)[2];  \
        (destino)[3] = (v1)[3] + (v2)[3];}

#define sqsADDVECTOR3(destino,v1,v2){  \
        (destino)[0] = (v1)[0] + (v2)[0];  \
        (destino)[1] = (v1)[1] + (v2)[1];  \
        (destino)[2] = (v1)[2] + (v2)[2];}

#define sqsSUBVECTOR4(destino,v1,v2){  \
        (destino)[0] = (v1)[0] - (v2)[0];  \
        (destino)[1] = (v1)[1] - (v2)[1];  \
        (destino)[2] = (v1)[2] - (v2)[2];  \
        (destino)[3] = (v1)[3] - (v2)[3];}

#define sqsSUBVECTOR3(destino,v1,v2){  \
        (destino)[0] = (v1)[0] - (v2)[0];  \
        (destino)[1] = (v1)[1] - (v2)[1];  \
        (destino)[2] = (v1)[2] - (v2)[2];}

#define sqsSCALEVECTOR3(destino,s,src){  \
        (destino)[0] = s*(src)[0];  \
        (destino)[1] = s*(src)[1];  \
        (destino)[2] = s*(src)[2];}

#define sqsSCALEVECTOR4(destino,s,src){  \
        (destino)[0] = s*(src)[0];  \
        (destino)[1] = s*(src)[1];  \
        (destino)[2] = s*(src)[2];  \
        (destino)[3] = s*(src)[3];}

#endif // GEOMETRY_H_INCLUDED
